﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

using xna = Microsoft.Xna.Framework;
using Microsoft.Xna.Framework;
using URWPGSim2D.Common;
using URWPGSim2D.StrategyLoader;

namespace URWPGSim2D.Strategy
{
    public class Strategy : MarshalByRefObject, IStrategy
    {
        #region reserved code never be changed or removed
        /// <summary>
        /// override the InitializeLifetimeService to return null instead of a valid ILease implementation
        /// to ensure this type of remote object never dies
        /// </summary>
        /// <returns>null</returns>
        public override object InitializeLifetimeService()
        {
            //return base.InitializeLifetimeService();
            return null; // makes the object live indefinitely
        }
        #endregion

        /// <summary>
        /// 决策类当前对象对应的仿真使命参与队伍的决策数组引用 第一次调用GetDecision时分配空间
        /// </summary>
        /// 

        private Decision[] decisions = null;

        /// <summary>
        /// 获取队伍名称 在此处设置参赛队伍的名称
        /// </summary>
        /// <returns>队伍名称字符串</returns>
        public string GetTeamName()
        {
            return "北信抢球博弈";
        }
        #region 系统变量声明
        public static float distanceOfBallToHole;                                                //球到洞口的距离（洞口点人为设定）
        public static float distanceOfFishPolToBall;                                            //鱼头到球前目标击球点的距离（这里的球前目标击球点在球内部（球内部位置点人为设定））
        private static float[] notesnega = new float[0x18];
        private static float[] notesplus = new float[0x18];
        public static float rd = 28f;
        public static float rdx;                                                                            //根据distanceOfBallToHole距离和rd+x算出的x轴分量
        public static float rdz;                                                                            //根据distanceOfBallToHole距离和rd+x算出的z轴分量
        public static float ballToHoleAngle;                                                        //球到洞内点正切角度值（洞内点人为设定）
        public static float fishPolToAheadOfBallAngle;
        public double[] TTable = new double[] { -0.39399999380111694, -0.296999990940094, -0.24400000274181366,
                                                -0.19200000166893005, -0.13699999451637268, -0.086999997496604919, 
                                                -0.052000001072883606, 0.0, 0.052000001072883606, 0.086999997496604919,
                                                0.13699999451637268, 0.19200000166893005, 0.24400000274181366,
                                                0.296999990940094, 0.34900000691413879, 0.34900000691413879 };
        public double[] VTable = new double[] { 0.0, 10.0, 35.0, 67.0, 98.0, 112.0, 135.0, 154.0, 175.0, 227.0, 273.0, 291.0, 298.0, 294.0, 307.0, 317.0 };
        public static float[] VCodeAndVelocityTable = new float[16] { 0, 0, 0.091f, 67, 112, 112, 112, 154, 175, 227, 273, 291, 298, 294, 307, 317 };
        public static float[] TCodeAndAngularVelocityTable = new float[16] { -3.000f, -0.788f, -0.634f, -0.535f, -0.400f, -0.356f, -0.088f, 0, 0.088f, 0.134f, 0.289f, 0.356f, 0.400f, 0.535f, 0.634f, 3.000f };

        #endregion

        #region 自定义条件声明

        float fishTopiontAngle1 = 0;        //鱼1到对应点的角度
        float fishTopiontAngle2 = 0;        //鱼2到对应点的角度
        int myHalfCourt = 0;//默认左半场为主场


        xna.Vector3 Point00 = new xna.Vector3(150, 0, -20);
        int flag00 = 0;
        xna.Vector3 Point01 = new xna.Vector3(-200, 0, -400);
        int flag01 = 0;
        xna.Vector3 Point02 = new xna.Vector3(-1300, 0, -780);
        int flag02 = 0;
        xna.Vector3 Point03 = new xna.Vector3(-1300, 0, -400);
        int flag03 = 0;
        xna.Vector3 Point04 = new xna.Vector3(-1080, 0, -400);
        int flag04 = 0;
        xna.Vector3 Point05 = new xna.Vector3(-1300, 0, 700);
        int flag05 = 0;

        int miao = 0;

        #endregion



        /// <summary>
        /// 获取当前仿真使命（比赛项目）当前队伍所有仿真机器鱼的决策数据构成的数组
        /// </summary>
        /// <param name="mission">服务端当前运行着的仿真使命Mission对象</param>
        /// <param name="teamId">当前队伍在服务端运行着的仿真使命中所处的编号 
        /// 用于作为索引访问Mission对象的TeamsRef队伍列表中代表当前队伍的元素</param>
        /// <returns>当前队伍所有仿真机器鱼的决策数据构成的Decision数组对象</returns>
        public Decision[] GetDecision(Mission mission, int teamId)
        {
            // 决策类当前对象第一次调用GetDecision时Decision数组引用为null
            if (decisions == null)
            {// 根据决策类当前对象对应的仿真使命参与队伍仿真机器鱼的数量分配决策数组空间
                decisions = new Decision[mission.CommonPara.FishCntPerTeam];
            }

            #region 决策计算过程 需要各参赛队伍实现的部分
            #region 策略编写帮助信息
            //====================我是华丽的分割线====================//
            //======================策略编写指南======================//
            //1.策略编写工作直接目标是给当前队伍决策数组decisions各元素填充决策值
            //2.决策数据类型包括两个int成员，VCode为速度档位值，TCode为转弯档位值
            //3.VCode取值范围0-14共15个整数值，每个整数对应一个速度值，速度值整体但非严格递增
            //有个别档位值对应的速度值低于比它小的档位值对应的速度值，速度值数据来源于实验
            //4.TCode取值范围0-14共15个整数值，每个整数对应一个角速度值
            //整数7对应直游，角速度值为0，整数6-0，8-14分别对应左转和右转，偏离7越远，角度速度值越大
            //5.任意两个速度/转弯档位之间切换，都需要若干个仿真周期，才能达到稳态速度/角速度值
            //目前运动学计算过程决定稳态速度/角速度值接近但小于目标档位对应的速度/角速度值
            //6.决策类Strategy的实例在加载完毕后一直存在于内存中，可以自定义私有成员变量保存必要信息
            //但需要注意的是，保存的信息在中途更换策略时将会丢失
            //====================我是华丽的分割线====================//
            //=======策略中可以使用的比赛环境信息和过程信息说明=======//
            //场地坐标系: 以毫米为单位，矩形场地中心为原点，向右为正X，向下为正Z
            //            负X轴顺时针转回负X轴角度范围为(-PI,PI)的坐标系，也称为世界坐标系
            //mission.CommonPara: 当前仿真使命公共参数
            //mission.CommonPara.FishCntPerTeam: 每支队伍仿真机器鱼数量
            //mission.CommonPara.MsPerCycle: 仿真周期毫秒数
            //mission.CommonPara.RemainingCycles: 当前剩余仿真周期数
            //mission.CommonPara.TeamCount: 当前仿真使命参与队伍数量
            //mission.CommonPara.TotalSeconds: 当前仿真使命运行时间秒数
            //mission.EnvRef.Balls: 
            //当前仿真使命涉及到的仿真水球列表，列表元素的成员意义参见URWPGSim2D.Common.Ball类定义中的注释
            //mission.EnvRef.FieldInfo: 
            //当前仿真使命涉及到的仿真场地，各成员意义参见URWPGSim2D.Common.Field类定义中的注释
            //mission.EnvRef.ObstaclesRect: 
            //当前仿真使命涉及到的方形障碍物列表，列表元素的成员意义参见URWPGSim2D.Common.RectangularObstacle类定义中的注释
            //mission.EnvRef.ObstaclesRound:
            //当前仿真使命涉及到的圆形障碍物列表，列表元素的成员意义参见URWPGSim2D.Common.RoundedObstacle类定义中的注释
            //mission.TeamsRef[teamId]:
            //决策类当前对象对应的仿真使命参与队伍（当前队伍）
            //mission.TeamsRef[teamId].Para:
            //当前队伍公共参数，各成员意义参见URWPGSim2D.Common.TeamCommonPara类定义中的注释
            //mission.TeamsRef[teamId].Fishes:
            //当前队伍仿真机器鱼列表，列表元素的成员意义参见URWPGSim2D.Common.RoboFish类定义中的注释
            //mission.TeamsRef[teamId].Fishes[i].PositionMm和PolygonVertices[0],BodyDirectionRad,VelocityMmPs,
            //                                   AngularVelocityRadPs,Tactic:
            //当前队伍第i条仿真机器鱼鱼体矩形中心和鱼头顶点在场地坐标系中的位置（用到X坐标和Z坐标），鱼体方向，速度值，
            //                                   角速度值，决策值
            //====================我是华丽的分割线====================//
            //========================典型循环========================//
            //for (int i = 0; i < mission.CommonPara.FishCntPerTeam; i++)
            //{
            //  decisions[i].VCode = 0; // 静止
            //  decisions[i].TCode = 7; // 直游
            //}
            //====================我是华丽的分割线====================//
            #endregion
            //请从这里开始编写代码

            #region 定义鱼与球的信息

            bool fish1 = true;//我方鱼1开始运动
            bool fish2 = true;//我方鱼2开始运动

            RoboFish myFish1 = mission.TeamsRef[teamId].Fishes[0];      //我方鱼1
            RoboFish myFish2 = mission.TeamsRef[teamId].Fishes[1];      //我方鱼2

            Ball ball1 = mission.EnvRef.Balls[0];                       //球3
            Ball ball2 = mission.EnvRef.Balls[1];                       //球2
            Ball ball3 = mission.EnvRef.Balls[2];                       //球3
            Ball ball4 = mission.EnvRef.Balls[3];                       //球4
            Ball ball5 = mission.EnvRef.Balls[4];                       //球5
            Ball ball6 = mission.EnvRef.Balls[5];                       //球6
            Ball ball7 = mission.EnvRef.Balls[6];                       //球7
            Ball ball8 = mission.EnvRef.Balls[7];                       //球8
            Ball ball9 = mission.EnvRef.Balls[8];                       //球9


            xna.Vector3 myFish1CenterPosition = mission.TeamsRef[teamId].Fishes[0].PositionMm;                          //我方鱼1中心点坐标
            xna.Vector3 myFish1HeadPosition = mission.TeamsRef[teamId].Fishes[0].PolygonVertices[0];                    //我方鱼1鱼头部坐标
            xna.Vector3 myFish1LeftHeadPosition = mission.TeamsRef[teamId].Fishes[0].PolygonVertices[1];                //我方鱼1左边鱼头部坐标
            xna.Vector3 myFish1LeftFinPosition = mission.TeamsRef[teamId].Fishes[0].PolygonVertices[2];                 //我方鱼1左胸鳍坐标
            xna.Vector3 myFish1LeftTailPosition = mission.TeamsRef[teamId].Fishes[0].PolygonVertices[3];                //我方鱼1左尾第三关节坐标
            xna.Vector3 myFish1RightTailPosition = mission.TeamsRef[teamId].Fishes[0].PolygonVertices[4];               //我方鱼1右尾第三关节坐标           
            xna.Vector3 myFish1RightFinPosition = mission.TeamsRef[teamId].Fishes[0].PolygonVertices[5];                //我方鱼1右胸鳍坐标
            xna.Vector3 myFish1RightHeadPosition = mission.TeamsRef[teamId].Fishes[0].PolygonVertices[6];               //我方鱼1右边鱼头部坐标
            float myFish1BodyDirectionRad = mission.TeamsRef[teamId].Fishes[0].BodyDirectionRad;                        //我方鱼1鱼体方向

            xna.Vector3 myFish2CenterPosition = mission.TeamsRef[teamId].Fishes[1].PositionMm;                          //我方鱼2中心点坐标
            xna.Vector3 myFish2HeadPosition = mission.TeamsRef[teamId].Fishes[1].PolygonVertices[0];                    //我方鱼2鱼头部坐标
            xna.Vector3 myFish2LeftHeadPosition = mission.TeamsRef[teamId].Fishes[1].PolygonVertices[1];                //我方鱼2左边鱼头部坐标
            xna.Vector3 myFish2LeftFinPosition = mission.TeamsRef[teamId].Fishes[1].PolygonVertices[2];                 //我方鱼2左胸鳍坐标
            xna.Vector3 myFish2LeftTailPosition = mission.TeamsRef[teamId].Fishes[1].PolygonVertices[3];                //我方鱼2左尾第三关节坐标
            xna.Vector3 myFish2RightTailPosition = mission.TeamsRef[teamId].Fishes[1].PolygonVertices[4];               //我方鱼2右尾第三关节坐标           
            xna.Vector3 myFish2RightFinPosition = mission.TeamsRef[teamId].Fishes[1].PolygonVertices[5];                //我方鱼2右胸鳍坐标
            xna.Vector3 myFish2RightHeadPosition = mission.TeamsRef[teamId].Fishes[1].PolygonVertices[6];               //我方鱼2右边鱼头部坐标
            float myFish2BodyDirectionRad = mission.TeamsRef[teamId].Fishes[0].BodyDirectionRad;                        //我方鱼2鱼体方向

            xna.Vector3 enemyFish1CenterPosition = mission.TeamsRef[(teamId + 1) % 2].Fishes[0].PositionMm;             //敌方鱼1中心点坐标
            xna.Vector3 enemyFish1HeadPosition = mission.TeamsRef[(teamId + 1) % 2].Fishes[0].PolygonVertices[0];       //敌方鱼1鱼头部坐标
            xna.Vector3 enemyFish1LeftHeadPosition = mission.TeamsRef[(teamId + 1) % 2].Fishes[0].PolygonVertices[1];   //敌方鱼1左边鱼头部坐标
            xna.Vector3 enemyFish1LeftFinPosition = mission.TeamsRef[(teamId + 1) % 2].Fishes[0].PolygonVertices[2];    //敌方鱼1左胸鳍坐标
            xna.Vector3 enemyFish1LeftTailPosition = mission.TeamsRef[(teamId + 1) % 2].Fishes[0].PolygonVertices[3];   //敌方鱼1左尾第三关节坐标
            xna.Vector3 enemyFish1RightTailPosition = mission.TeamsRef[(teamId + 1) % 2].Fishes[0].PolygonVertices[4];  //敌方鱼1右尾第三关节坐标           
            xna.Vector3 enemyFish1RightFinPosition = mission.TeamsRef[(teamId + 1) % 2].Fishes[0].PolygonVertices[5];   //敌方鱼1右胸鳍坐标
            xna.Vector3 enemyFish1RightHeadPosition = mission.TeamsRef[(teamId + 1) % 2].Fishes[0].PolygonVertices[6];  //敌方鱼1右边鱼头部坐标
            float enemyFish1BodyDirectionRad = mission.TeamsRef[(teamId + 1) % 2].Fishes[0].BodyDirectionRad;           //敌方鱼1鱼体方向

            xna.Vector3 enemyFish2CenterPosition = mission.TeamsRef[(teamId + 1) % 2].Fishes[1].PositionMm;             //敌方鱼2中心点坐标
            xna.Vector3 enemyFish2HeadPosition = mission.TeamsRef[(teamId + 1) % 2].Fishes[1].PolygonVertices[0];       //敌方鱼2鱼头部坐标
            xna.Vector3 enemyFish2LeftHeadPosition = mission.TeamsRef[(teamId + 1) % 2].Fishes[1].PolygonVertices[1];   //敌方鱼2左边鱼头部坐标
            xna.Vector3 enemyFish2LeftFinPosition = mission.TeamsRef[(teamId + 1) % 2].Fishes[1].PolygonVertices[2];    //敌方鱼2左胸鳍坐标
            xna.Vector3 enemyFish2LeftTailPosition = mission.TeamsRef[(teamId + 1) % 2].Fishes[1].PolygonVertices[3];   //敌方鱼2左尾第三关节坐标
            xna.Vector3 enemyFish2RightTailPosition = mission.TeamsRef[(teamId + 1) % 2].Fishes[1].PolygonVertices[4];  //敌方鱼2右尾第三关节坐标           
            xna.Vector3 enemyFish2RightFinPosition = mission.TeamsRef[(teamId + 1) % 2].Fishes[1].PolygonVertices[5];   //敌方鱼2右胸鳍坐标
            xna.Vector3 enemyFish2RightHeadPosition = mission.TeamsRef[(teamId + 1) % 2].Fishes[1].PolygonVertices[6];  //敌方鱼2右边鱼头部坐标
            float enemyFish2BodyDirectionRad = mission.TeamsRef[(teamId + 1) % 2].Fishes[1].BodyDirectionRad;           //敌方鱼2鱼体方向

            xna.Vector3 ball1Position = mission.EnvRef.Balls[0].PositionMm;                                             //球3坐标
            xna.Vector3 ball2Position = mission.EnvRef.Balls[1].PositionMm;                                             //球2坐标
            xna.Vector3 ball3Position = mission.EnvRef.Balls[2].PositionMm;                                             //球3坐标
            xna.Vector3 ball4Position = mission.EnvRef.Balls[3].PositionMm;                                             //球4坐标
            xna.Vector3 ball5Position = mission.EnvRef.Balls[4].PositionMm;                                             //球5坐标
            xna.Vector3 ball6Position = mission.EnvRef.Balls[5].PositionMm;                                             //球6坐标
            xna.Vector3 ball7Position = mission.EnvRef.Balls[6].PositionMm;                                             //球7坐标
            xna.Vector3 ball8Position = mission.EnvRef.Balls[7].PositionMm;                                             //球8坐标
            xna.Vector3 ball9Position = mission.EnvRef.Balls[8].PositionMm;                                             //球9坐标

            xna.Vector3 A1 = new Vector3(-1375, 0, -875);                    //A1中心坐标
            xna.Vector3 A2 = new Vector3(-1375, 0, -650);                    //A2中心坐标
            xna.Vector3 A3 = new Vector3(-1375, 0, -275);                    //A3中心坐标
            xna.Vector3 A4 = new Vector3(-1375, 0, +275);                    //A4中心坐标
            xna.Vector3 A5 = new Vector3(-1375, 0, +650);                    //A5中心坐标
            xna.Vector3 A6 = new Vector3(-1375, 0, +875);                    //A6中心坐标

            xna.Vector3 B1 = new Vector3(-1100, 0, -875);                    //B1中心坐标
            xna.Vector3 B2 = new Vector3(-1100, 0, -650);                    //B2中心坐标
            xna.Vector3 B3 = new Vector3(-1100, 0, -275);                    //B3中心坐标
            xna.Vector3 B4 = new Vector3(-1100, 0, +275);                    //B4中心坐标
            xna.Vector3 B5 = new Vector3(-1100, 0, +650);                    //B5中心坐标
            xna.Vector3 B6 = new Vector3(-1100, 0, +875);                    //B6中心坐标

            xna.Vector3 C1 = new Vector3(-750, 0, -875);                     //C1中心坐标
            xna.Vector3 C2 = new Vector3(-750, 0, -650);                     //C2中心坐标
            xna.Vector3 C3 = new Vector3(-750, 0, -375);                     //C3中心坐标
            xna.Vector3 C4 = new Vector3(-750, 0, 0);                        //C4中心坐标
            xna.Vector3 C5 = new Vector3(-750, 0, +375);                     //C5中心坐标
            xna.Vector3 C6 = new Vector3(-750, 0, +650);                     //C6中心坐标
            xna.Vector3 C7 = new Vector3(-750, 0, +875);                     //C7中心坐标

            xna.Vector3 D1 = new Vector3(0, 0, -875);                        //D1中心坐标
            xna.Vector3 D2 = new Vector3(0, 0, -650);                        //D2中心坐标
            xna.Vector3 D3 = new Vector3(0, 0, -375);                        //D3中心坐标
            xna.Vector3 D4 = new Vector3(0, 0, 0);                           //D4中心坐标
            xna.Vector3 D5 = new Vector3(0, 0, +375);                        //D5中心坐标
            xna.Vector3 D6 = new Vector3(0, 0, +650);                        //D6中心坐标
            xna.Vector3 D7 = new Vector3(0, 0, +875);                        //D7中心坐标

            xna.Vector3 E1 = new Vector3(750, 0, -875);                      //E1中心坐标
            xna.Vector3 E2 = new Vector3(750, 0, -650);                      //E2中心坐标
            xna.Vector3 E3 = new Vector3(750, 0, -375);                      //E3中心坐标
            xna.Vector3 E4 = new Vector3(750, 0, 0);                         //E4中心坐标
            xna.Vector3 E5 = new Vector3(750, 0, +375);                      //E5中心坐标
            xna.Vector3 E6 = new Vector3(750, 0, +650);                      //E6中心坐标
            xna.Vector3 E7 = new Vector3(750, 0, +875);                      //E7中心坐标

            xna.Vector3 F1 = new Vector3(1100, 0, -875);                     //F1中心坐标
            xna.Vector3 F2 = new Vector3(1100, 0, -650);                     //F2中心坐标
            xna.Vector3 F3 = new Vector3(1100, 0, -275);                     //F3中心坐标
            xna.Vector3 F4 = new Vector3(1100, 0, +275);                     //F4中心坐标
            xna.Vector3 F5 = new Vector3(1100, 0, +650);                     //F5中心坐标
            xna.Vector3 F6 = new Vector3(1100, 0, +875);                     //F6中心坐标

            xna.Vector3 G1 = new Vector3(1375, 0, -875);                     //G1中心坐标
            xna.Vector3 G2 = new Vector3(1375, 0, -650);                     //G2中心坐标
            xna.Vector3 G3 = new Vector3(1375, 0, -275);                     //G3中心坐标
            xna.Vector3 G4 = new Vector3(1375, 0, +275);                     //G4中心坐标
            xna.Vector3 G5 = new Vector3(1375, 0, +650);                     //G5中心坐标
            xna.Vector3 G6 = new Vector3(1375, 0, +875);                     //G6中心坐标

            xna.Vector3 LG = new Vector3(-950, 0, 0);                     //左球门中心坐标
            xna.Vector3 RG = new Vector3(+950, 0, 0);                     //有球门中心坐标

            #endregion




            #region 进球情况
            //值取1为进球,值取0为未进球
            int b0_l = Convert.ToInt32(mission.HtMissionVariables["Ball_0_Left_Status"]);                               //左方球门球3进球情况
            int b1_l = Convert.ToInt32(mission.HtMissionVariables["Ball_1_Left_Status"]);                               //左方球门球2进球情况
            int b2_l = Convert.ToInt32(mission.HtMissionVariables["Ball_2_Left_Status"]);                               //左方球门球3进球情况
            int b3_l = Convert.ToInt32(mission.HtMissionVariables["Ball_3_Left_Status"]);                               //左方球门球4进球情况
            int b4_l = Convert.ToInt32(mission.HtMissionVariables["Ball_4_Left_Status"]);                               //左方球门球5进球情况
            int b5_l = Convert.ToInt32(mission.HtMissionVariables["Ball_5_Left_Status"]);                               //左方球门球6进球情况
            int b6_l = Convert.ToInt32(mission.HtMissionVariables["Ball_6_Left_Status"]);                               //左方球门球7进球情况
            int b7_l = Convert.ToInt32(mission.HtMissionVariables["Ball_7_Left_Status"]);                               //左方球门球8进球情况
            int b8_l = Convert.ToInt32(mission.HtMissionVariables["Ball_8_Left_Status"]);                               //左方球门球9进球情况
            int b0_r = Convert.ToInt32(mission.HtMissionVariables["Ball_0_Right_Status"]);                              //右方球门球3进球情况
            int b1_r = Convert.ToInt32(mission.HtMissionVariables["Ball_1_Right_Status"]);                              //右方球门球2进球情况
            int b2_r = Convert.ToInt32(mission.HtMissionVariables["Ball_2_Right_Status"]);                              //右方球门球3进球情况
            int b3_r = Convert.ToInt32(mission.HtMissionVariables["Ball_3_Right_Status"]);                              //右方球门球4进球情况
            int b4_r = Convert.ToInt32(mission.HtMissionVariables["Ball_4_Right_Status"]);                              //右方球门球5进球情况
            int b5_r = Convert.ToInt32(mission.HtMissionVariables["Ball_5_Right_Status"]);                              //右方球门球6进球情况
            int b6_r = Convert.ToInt32(mission.HtMissionVariables["Ball_6_Right_Status"]);                              //右方球门球7进球情况
            int b7_r = Convert.ToInt32(mission.HtMissionVariables["Ball_7_Right_Status"]);                              //右方球门球8进球情况
            int b8_r = Convert.ToInt32(mission.HtMissionVariables["Ball_8_Right_Status"]);                              //右方球门球9进球情况

            #endregion

            #region 半场分配

            if (mission.TeamsRef[teamId].Para.MyHalfCourt == HalfCourt.RIGHT)
            {   //在右半场
                myHalfCourt = 1;
            }
            else
            {   //在左半场
                myHalfCourt = 0;
            }
            #endregion
              
            #region   在这里编写你的策略

            if (miao == 0)
            {
                //StrategyHelper.Helpers.Dribble(ref decisions[1], mission.TeamsRef[teamId].Fishes[1], Point00, 0, 5, 10, 150, 15, 12, 15, 100, false);
                Dribbleexptop(ref decisions[1], myFish2, 5, 10, 150, 15, 12, 15, 100, false, Point00);
                if (myFish2.PolygonVertices[0].X >= 130)
                {
                    flag00 = 1;
                }
                if (flag00 == 1)
                {
                    HSgo(ref decisions[1], myFish2, Point01, ball1, 0f, 3);
                    if (ball1.PositionMm.X <= -200 || ball1.PositionMm.Z <= -350)
                    {
                        flag01 = 1;
                    }
                    if (flag01 == 1)
                    {
                        mydrible(ref decisions[1], myFish2, Point02, ball1);
                        if (ball1.PositionMm.X <= -1300)
                        {
                            flag02 = 1;
                        }
                        if (flag02 == 1)
                        {
                            HSgo(ref decisions[1], myFish2, Point03, ball1, 0f, 1);
                            if (ball1.PositionMm.Z >= -400)
                            {
                                flag03 = 1;
                            }
                            if (flag03 == 1)
                            {
                                HSgo(ref decisions[1], myFish2, Point04, ball1, 0f, 2);
                                if (ball1.PositionMm.X >= -1080)
                                {
                                    flag04 = 1;
                                }
                                if (flag04 == 1)
                                {
                                    Dribble(ref decisions[1], myFish2, Point05, 0, 5, 10, 150, 15, 12, 15, 100, false);
                                    if (myFish2.PolygonVertices[0].Z >= 700)//结束
                                    {
                                        flag05 = 1;
                                    }
                                    if (flag05 == 1)
                                    {
                                        decisions[1].VCode = 7;//线速度
                                        decisions[1].TCode = 7;//角速度
                                    }
                                }
                            }
                        }
                    }
                }

            }



            #endregion

            return decisions;
        }


        #region 相关函数
        #region distancetoangle返回两点间正切角度值（方向值）
        private float distancetoangle(float x, float z, float a, float b)
        {
            float num = b - z;
            float num2 = a - x;
            return (float)Math.Atan((double)(num / num2));
        }
        #endregion
        #region GetAngleToDestpoint2D返回当前点到目标点的正切角度值（方向值）
        private float GetAngleToDestpoint2D(float cur_x, float cur_z, float dest_x, float dest_z)
        {
            return (float)Math.Atan2((double)(dest_z - cur_z), (double)(dest_x - cur_x));
        }
        #endregion
        #region ReturnAngle返回角度
        private float ReturnAngle(float cur_x, float cur_z, float dest_x, float dest_z, float fish_rad)
        {
            float num = (float)Math.Abs(Math.Atan((double)((cur_x - dest_x) / (cur_z - dest_z))));
            if ((cur_x > dest_x) && (cur_z > dest_z))
            {
                if ((fish_rad > -(1.5707963267948966 + num)) && (fish_rad < (1.5707963267948966 - num)))
                {
                    fishPolToAheadOfBallAngle = (((float)1.5707963267948966) + num) + fish_rad;
                    fishPolToAheadOfBallAngle = -fishPolToAheadOfBallAngle;
                }
                else if ((fish_rad > (1.5707963267948966 - num)) && (fish_rad < 3.1415926535897931))
                {
                    fishPolToAheadOfBallAngle = (((float)4.71238898038469) + fish_rad) - num;
                }
                else if ((fish_rad < -(1.5707963267948966 + num)) && (fish_rad > -3.1415926535897931))
                {
                    fishPolToAheadOfBallAngle = (((float)-1.5707963267948966) - num) - fish_rad;
                }
            }
            else if ((cur_x > dest_x) && (cur_z < dest_z))
            {
                if ((fish_rad < (1.5707963267948966 + num)) && ((1.5707963267948966 - num) < fish_rad))
                {
                    fishPolToAheadOfBallAngle = (((float)1.5707963267948966) + num) - fish_rad;
                }
                else if (((1.5707963267948966 - num) > fish_rad) && (fish_rad > -3.1415926535897931))
                {
                    fishPolToAheadOfBallAngle = (((float)4.71238898038469) + fish_rad) - num;
                    fishPolToAheadOfBallAngle = -fishPolToAheadOfBallAngle;
                }
                else if ((fish_rad > (1.5707963267948966 + num)) && (fish_rad < 3.1415926535897931))
                {
                    fishPolToAheadOfBallAngle = (fish_rad - num) - ((float)1.5707963267948966);
                    fishPolToAheadOfBallAngle = -fishPolToAheadOfBallAngle;
                }
            }
            else if ((cur_x < dest_x) && (cur_z > dest_z))
            {
                if ((fish_rad > -(1.5707963267948966 - num)) && (fish_rad < (1.5707963267948966 + num)))
                {
                    fishPolToAheadOfBallAngle = (((float)3.1415926535897931) + fish_rad) - num;
                    fishPolToAheadOfBallAngle = -fishPolToAheadOfBallAngle;
                }
                else if ((fish_rad < -(1.5707963267948966 - num)) && (fish_rad > -3.1415926535897931))
                {
                    fishPolToAheadOfBallAngle = (num - fish_rad) - ((float)1.5707963267948966);
                }
                else if ((fish_rad > (1.5707963267948966 + num)) && (fish_rad < 3.1415926535897931))
                {
                    fishPolToAheadOfBallAngle = (((float)4.71238898038469) + num) - fish_rad;
                }
            }
            else if ((cur_x < dest_x) && (cur_z < dest_z))
            {
                if ((fish_rad < (1.5707963267948966 - num)) && (fish_rad > -(1.5707963267948966 + num)))
                {
                    fishPolToAheadOfBallAngle = (((float)1.5707963267948966) - num) - fish_rad;
                }
                else if ((fish_rad > (1.5707963267948966 - num)) && (fish_rad < 3.1415926535897931))
                {
                    fishPolToAheadOfBallAngle = (fish_rad + num) - ((float)1.5707963267948966);
                    fishPolToAheadOfBallAngle = -fishPolToAheadOfBallAngle;
                }
                else if ((fish_rad < -(1.5707963267948966 + num)) && (fish_rad > -3.1415926535897931))
                {
                    fishPolToAheadOfBallAngle = (((float)4.71238898038469) + fish_rad) + num;
                    fishPolToAheadOfBallAngle = -fishPolToAheadOfBallAngle;
                }
            }
            return fishPolToAheadOfBallAngle;
        }
        #endregion
        #region TransAngVeltoTCode控制返回线速度调整档位
        private int TransAngVeltoTCode(float angvel)
        {
            if (0f == angvel)
            {
                return 7;
            }
            if (0f > angvel)
            {
                if ((-0.088 <= angvel) && (0f > angvel))
                {
                    return 6;
                }
                if ((-0.356 <= angvel) && (-0.088 > angvel))
                {
                    return 5;
                }
                if ((-0.4 <= angvel) && (-0.356 > angvel))
                {
                    return 4;
                }
                if ((-0.535 <= angvel) && (-0.4 > angvel))
                {
                    return 3;
                }
                if ((-0.634 <= angvel) && (-0.535 > angvel))
                {
                    return 2;
                }
                if ((-0.788 <= angvel) && (-0.634 > angvel))
                {
                    return 1;
                }
                return 0;
            }
            if ((0.088 >= angvel) && (0f < angvel))
            {
                return 8;
            }
            if ((0.134 >= angvel) && (0.088 < angvel))
            {
                return 9;
            }
            if ((0.289 >= angvel) && (0.134 < angvel))
            {
                return 10;
            }
            if ((0.356 >= angvel) && (0.289 < angvel))
            {
                return 11;
            }
            if ((0.4 >= angvel) && (0.356 < angvel))
            {
                return 12;
            }
            if ((0.535 >= angvel) && (0.4 < angvel))
            {
                return 13;
            }
            if ((0.634 >= angvel) && (0.535 < angvel))
            {
                return 14;
            }
            return 15;
        }
        #endregion
        #region TransfromAngletoTCode控制返回角速度调整档位
        private int TransfromAngletoTCode(float angvel)
        {
            if (0f == angvel)
            {
                return 7;
            }
            if (angvel < 0f)
            {
                if ((-0.005395 <= angvel) && (0f > angvel))
                {
                    if ((0f - angvel) >= (angvel + 0.005395))
                    {
                        return 6;
                    }
                    return 7;
                }
                if ((-0.009016 <= angvel) && (-0.005395 > angvel))
                {
                    if ((-0.005395 - angvel) >= (angvel + 0.009016))
                    {
                        return 5;
                    }
                    return 6;
                }
                if ((-0.014203 <= angvel) && (-0.009016 > angvel))
                {
                    if ((-0.009016 - angvel) >= (angvel + 0.014203))
                    {
                        return 4;
                    }
                    return 5;
                }
                if ((-0.019907 <= angvel) && (-0.014203 > angvel))
                {
                    if ((-0.014203 - angvel) >= (angvel + 0.019907))
                    {
                        return 3;
                    }
                    return 4;
                }
                if ((-0.0253 <= angvel) && (-0.019907 > angvel))
                {
                    if ((-0.019907 - angvel) >= (angvel + 0.0253))
                    {
                        return 2;
                    }
                    return 3;
                }
                if ((-0.033592 > angvel) || (-0.0253 <= angvel))
                {
                    return 0;
                }
                if ((-0.0253 - angvel) >= (angvel + 0.033592))
                {
                    return 1;
                }
                return 2;
            }
            if ((0.005395 >= angvel) && (0f < angvel))
            {
                if ((angvel - 0f) > (0.005395 - angvel))
                {
                    return 8;
                }
                return 7;
            }
            if ((0.009016 >= angvel) && (0.005395 < angvel))
            {
                if ((angvel - 0.005395) > (0.009016 - angvel))
                {
                    return 9;
                }
                return 8;
            }
            if ((0.014203 >= angvel) && (0.009016 < angvel))
            {
                if ((angvel - 0.009016) > (0.014203 - angvel))
                {
                    return 10;
                }
                return 9;
            }
            if ((0.019907 >= angvel) && (0.014203 < angvel))
            {
                if ((angvel - 0.014203) > (0.019907 - angvel))
                {
                    return 11;
                }
                return 10;
            }
            if ((0.0253 >= angvel) && (0.019907 < angvel))
            {
                if ((angvel - 0.019907) > (0.0253 - angvel))
                {
                    return 12;
                }
                return 11;
            }
            if ((0.033592 >= angvel) && (0.0253 < angvel))
            {
                if ((angvel - 0.0253) > (0.033592 - angvel))
                {
                    return 13;
                }
                return 12;
            }
            if ((0.040848 < angvel) || (0.033592 >= angvel))
            {
                return 15;
            }
            if ((angvel - 0.033592) > (0.040848 - angvel))
            {
                return 14;
            }
            return 13;
        }
        #endregion
        #region GetLengthToDestpoint
        private float GetLengthToDestpoint(float cur_x, float cur_z, float dest_x, float dest_z)
        {
            return (float)Math.Sqrt(Math.Pow((double)(cur_x - dest_x), 2.0) + Math.Pow((double)(cur_z - dest_z), 2.0));
        }
        #endregion
        #region GetRadius
        private float GetRadius(float cur_x, float cur_z, float dest_x, float dest_z, float cur_direc, float last_direc)
        {
            float num = this.GetLengthToDestpoint(cur_x, cur_z, dest_x, dest_z);
            float num2 = this.GetAngleToDestpoint2D(cur_x, cur_z, dest_x, dest_z) - cur_direc;
            if ((0f == num) || (0f == num2))
            {
                return 0f;
            }
            if ((num2 >= 0f) || (num2 <= -3.1415926535897931))
            {
                if ((num2 > 0f) && (num2 < 3.1415926535897931))
                {
                    return (float)Math.Abs((double)(((double)(num / 2f)) / Math.Sin((double)num2)));
                }
                if (num2 < -3.1415926535897931)
                {
                    return (float)Math.Abs((double)(((double)(num / 2f)) / Math.Sin((double)num2)));
                }
            }
            return -((float)Math.Abs((double)(((double)(num / 2f)) / Math.Sin((double)num2))));
        }
        #endregion
        #region GetAngleDegree
        /// <summary>
        /// 返回Vector3类型的向量（Y置0，只有X和Z有意义）在场地坐标系中方向的角度值 LiYoubing 20110722
        /// 场地坐标系定义为：X向右，Z向下，Y置0，负X轴顺时针转回负X轴角度范围为(-PI,PI)的坐标系
        /// </summary>
        /// <param name="v">待计算角度值的xna.Vector3类型向量</param>
        /// <returns>向量v在场地坐标系中方向的角度值</returns>
        public static float GetAngleDegree(xna.Vector3 v)
        {
            float x = v.X;
            float y = v.Z;
            float angle = 0;

            if (Math.Abs(x) < float.Epsilon)
            {// x = 0 直角反正切不存在
                if (Math.Abs(y) < float.Epsilon) { angle = 0.0f; }
                else if (y > 0) { angle = 90.0f; }
                else if (y < 0) { angle = -90.0f; }
            }
            else if (x < 0)
            {// x < 0 (90,180]或(-180,-90)
                if (y >= 0) { angle = (float)(180 * Math.Atan(y / x) / Math.PI) + 180.0f; }
                else { angle = (float)(180 * Math.Atan(y / x) / Math.PI) - 180.0f; }
            }
            else
            {// x > 0 (-90,90)
                angle = (float)(180 * Math.Atan(y / x) / Math.PI);
            }

            return angle;
        }
        #endregion
        #region GetAngleDegreel
        public static float GetAngleDegree1(xna.Vector3 v)
        {
            float x = v.X;
            float y = v.Z;
            float angle = 0;

            if (Math.Abs(x) < float.Epsilon)
            {// x = 0 直角反正切不存在
                if (Math.Abs(y) < float.Epsilon) { angle = 0.0f; }
                else if (y > 0) { angle = 90.0f; }
                else if (y < 0) { angle = -90.0f; }
            }
            else if (x < 0)
            {// x < 0 (90,180]或(-180,-90)
                if (y >= 0) { angle = (float)(180 * Math.Atan(y / x) / Math.PI) + 180.0f; }
                else { angle = (float)(180 * Math.Atan(y / x) / Math.PI) - 180.0f; }
            }
            else
            {// x > 0 (-90,90)
                angle = (float)(180 * Math.Atan(y / x) / Math.PI);
            }

            return angle;
        }
        #endregion
        #region dribble
        /// <summary>
        /// dirbble带球算法 修改 By yibu 2015.06.02
        /// </summary>
        /// <param name="decision">每周期机器鱼执行策略，包含速度档位，转弯档位。</param>
        /// <param name="fish">目标仿真机器鱼参数，包含当前位置、速度信息。</param>
        /// <param name="destPtMm">目标点。</param>
        /// <param name="destDirRad">目标方向。</param>
        /// <param name="angleTheta1">鱼体方向与目标方向角度差的阈值一。
        ///   角度差在此阈值范围内，则赋给机器鱼一个合理的速度档位（见参数disThreshold说明）。</param>
        /// <param name="angleTheta2">鱼体方向与目标方向角度差的阈值二。角度差小于此阈值，则机器鱼直线游动；
        /// 角度差大于此阈值，则机器鱼调整游动方向。</param>
        /// <param name="disThreshold">距离阈值。距离大于此阈值，机器鱼以速度档位VCode1游动；
        /// 距离小于此阈值，机器鱼以速度档位VCode2游动。</param>
        /// /// <param name="VCode1">直游档位1（默认6档）。</param>
        /// /// <param name="VCode2">直游档位2（默认4档）。</param>
        /// <param name="cycles">速度和转弯档位之间切换所需周期数经验值。建议取值范围在5-20之间。此参数作用是防止机器鱼“转过”。</param>
        /// <param name="msPerCycle">每个仿真周期的毫秒数，传递固定参数，不能修改。</param>
        /// <param name="flag">机器鱼坐标选择标准，true为PositionMm，即鱼体绘图中心；false为PolygonVertices[0]，即鱼头点。</param>
        public static void Dribble(ref Decision decision, RoboFish fish, xna.Vector3 destPtMm, float destDirRad,
            float angleTheta1, float angleTheta2, float disThreshold, int VCode1, int VCode2, int cycles, int msPerCycle, bool flag)
        {

            // 调节所用周期数及每周期毫秒数转换得到秒数
            double seconds1 = 15 * msPerCycle / 1000.0;
            double seconds2 = cycles * msPerCycle / 1000.0;
            // 标志量为true则起始点为PositionMm即鱼体绘图中心false则起始点为PolygonVertices[0]即鱼头点（起始点）
            xna.Vector3 srcPtMm = (flag == true) ? fish.PositionMm : fish.PolygonVertices[0];
            // 起始点到目标点的距离（目标距离）
            double disSrcPtMmToDestPtMm = Math.Sqrt(Math.Pow(destPtMm.X - srcPtMm.X, 2.0)
                + Math.Pow(destPtMm.Z - srcPtMm.Z, 2.0));

            // 鱼体绘图中心指向目标点向量方向的弧度值（中间方向）
            double dirFishToDestPtRad = xna.MathHelper.ToRadians((float)GetAngleDegree(destPtMm - fish.PositionMm));
            if (disSrcPtMmToDestPtMm < 30)
            {// 起始点到目标点距离小于阈值（默认58毫米）将中间方向调为目标方向
                dirFishToDestPtRad = destDirRad;
            }

            // 中间方向与鱼体方向的差值（目标角度）
            double deltaTheta = dirFishToDestPtRad - fish.BodyDirectionRad;
            // 将目标角度规范化到(-PI,PI]
            // 规范化之后目标角度为正表示目标方向在鱼体方向右边
            // 规范化之后目标角度为负表示目标方向在鱼体方向左边
            if (deltaTheta > Math.PI)
            {// 中间方向为正鱼体方向为负才可能目标角度大于PI
                deltaTheta -= 2 * Math.PI;  // 规范化到(-PI,0)
            }
            else if (deltaTheta < -Math.PI)
            {// 中间方向为负鱼体方向为正才可能目标角度小于-PI
                deltaTheta += 2 * Math.PI;  // 规范化到(0,PI)
            }

            // 最大角速度取左转和右转最大角速度绝对值的均值
            float maxAngularV = (Math.Abs(TCodeAndAngularVelocityTable[0])
                + Math.Abs(TCodeAndAngularVelocityTable[14])) / 2;
            // 以最大角速度转过目标角度所需的预计时间（角度预计时间）
            double estimatedTimeByAngle = Math.Abs((double)(deltaTheta / maxAngularV));
            // 以角度预计时间游过目标距离所需平均速度值（目标速度）
            double targetVelocity = disSrcPtMmToDestPtMm / estimatedTimeByAngle;

            int code = 1;   // 目标（速度）档位初值置1
            while ((code < 10) && (VCodeAndVelocityTable[code] < targetVelocity))
            {// 目标（速度）档位对应的速度值尚未达到目标速度则调高目标（速度）档位
                code++;
            }
            decision.VCode = code;
            if (Math.Abs(deltaTheta) > angleTheta2 * Math.PI / 180.0)
            {// 目标角度绝对值超过某一阈值，速度档位置次低进行小半径转弯
                decision.VCode = 1;
            }
            else if (Math.Abs(deltaTheta) < angleTheta1 * Math.PI / 180.0)
            {// 目标角度绝对值小于某一阈值，若此时距离较远速度档置较高高全速前进，否则置适中档位前进。
                if (disSrcPtMmToDestPtMm > disThreshold)
                {
                    decision.VCode = VCode1;
                }
                else
                {
                    decision.VCode = VCode2;
                }
            }

            // 以最大速度游过目标距离所需的预计时间（距离预计时间）
            double estimatedTimeByDistance = disSrcPtMmToDestPtMm / VCodeAndVelocityTable[14];
            if (estimatedTimeByDistance > seconds1)
            {// 距离预计时间超过一次档位切换所需平均时间则取为该时间（默认为1秒）
                estimatedTimeByDistance = seconds1;
            }
            // 以距离预计时间游过目标角度所需平均角速度（目标角速度）
            double targetAngularV = deltaTheta / estimatedTimeByDistance;

            code = 7;
            if (deltaTheta <= 0)
            {// 目标角度为负目标方向在鱼体方向左边需要给左转档位（注意左转档位对应的角速度值为负值）
                while ((code > 0) && (TCodeAndAngularVelocityTable[code] > targetAngularV))
                {// 目标（转弯）档位对应的角速度值尚未达到目标角速度则调低目标（转弯）档位
                    code--;
                }
                if ((fish.AngularVelocityRadPs * seconds2) < deltaTheta)
                {// 当前角速度值绝对值过大 一次档位切换所需平均时间内能游过的角度超过目标角度
                    // 则给相反方向次大转弯档位
                    code = 12;
                }
            }
            else
            {// 目标角度为正目标方向在鱼体方向右边需要给右转档位
                while ((code < 14) && (TCodeAndAngularVelocityTable[code] < targetAngularV))
                {// 目标（转弯）档位对应的角速度值尚未达到目标角速度则调高目标（转弯）档位
                    code++;
                }
                if ((fish.AngularVelocityRadPs * seconds2) > deltaTheta)
                {// 当前角速度值绝对值过大 一次档位切换所需平均时间内能游过的角度超过目标角度
                    // 则给相反方向次大转弯档位
                    code = 2;
                }
            }
            decision.TCode = code;
        }
        #endregion
        #region dribbleexptop
        /// <summary>
        /// dirbbleexptop 修改 By yibu 2015.06.02
        /// </summary>
        /// <param name="decision">每周期机器鱼执行策略，包含速度档位，转弯档位。</param>
        /// <param name="fish">目标仿真机器鱼参数，包含当前位置、速度信息。</param>
        /// <param name="destPtMm">目标点。</param>
        /// <param name="destDirRad">目标方向。</param>
        /// <param name="angleTheta1">鱼体方向与目标方向角度差的阈值一。
        ///   角度差在此阈值范围内，则赋给机器鱼一个合理的速度档位（见参数disThreshold说明）。</param>
        /// <param name="angleTheta2">鱼体方向与目标方向角度差的阈值二。角度差小于此阈值，则机器鱼直线游动；
        /// 角度差大于此阈值，则机器鱼调整游动方向。</param>
        /// <param name="disThreshold">距离阈值。距离大于此阈值，机器鱼以速度档位VCode1游动；
        /// 距离小于此阈值，机器鱼以速度档位VCode2游动。</param>
        /// /// <param name="VCode1">直游档位1（默认6档）。</param>
        /// /// <param name="VCode2">直游档位2（默认4档）。</param>
        /// <param name="cycles">速度和转弯档位之间切换所需周期数经验值。建议取值范围在5-20之间。此参数作用是防止机器鱼“转过”。</param>
        /// <param name="msPerCycle">每个仿真周期的毫秒数，传递固定参数，不能修改。</param>
        /// <param name="flag">机器鱼坐标选择标准，true为PositionMm，即鱼体绘图中心；false为PolygonVertices[0]，即鱼头点。</param>
        /// <param name="GoalPoint">机器鱼的顶球目标点。</param>
        /// <param name="ball">机器鱼的目标球。</param>
        public static void Dribbleexptop(ref Decision decision, RoboFish fish,
            float angleTheta1, float angleTheta2, float disThreshold, int VCode1, int VCode2, int cycles, int msPerCycle, bool flag, xna.Vector3 GoalPoint)
        {
            float dirBallToGoalRad1 = xna.MathHelper.ToRadians((float)GetAngleDegree1(GoalPoint - GoalPoint));
            // 球门正中进球点指向球心直线延长线与球边缘交点（目标点）

            Dribble(ref decision, fish, GoalPoint, dirBallToGoalRad1, angleTheta1, angleTheta2, disThreshold, VCode1, VCode2, cycles, msPerCycle, flag);

            //if ((Math.Abs(dirBallToFish1Rad - dirBallToGoalRad1) < MIN_DELTA_THETA) && Math.Abs(dirBallToFish1Rad - fish1.BodyDirectionRad) < MIN_DELTA_THETA)
            //{
            //   decisions[0].VCode = 10;
            //}


        }
        #endregion
        #region mydrible
        /*public void mydrible(ref Decision decision, RoboFish fish, Microsoft.Xna.Framework.Vector3 gate, Ball ball,Mission mission = null)
        {
            FileStream fs = new FileStream("test.txt", FileMode.Create, FileAccess.Write);
            StreamWriter sw = new StreamWriter(fs);
            sw.WriteLine("test\n");
            sw.Close();
            fs.Close();
           // decisions[0].VCode = 0;
           // decisions[0].TCode = 8;
            xna.Vector3 GP1 = new xna.Vector3(724, 0, -424);
            int times = 0;
            if (Math.Abs(fish.BodyDirectionRad - 1.57) > 0.051 || GetLengthToDestpoint(fish.PolygonVertices[0].X, fish.PolygonVertices[0].Z,
               724, -424 ) >= 5)
            {

                Helpers.PoseToPose(ref decisions[0], fish, GP1, 1.57f, 30.0f, 8 * ball.RadiusMm, mission.CommonPara.MsPerCycle, ref times);
            }
           // Helpers.PoseToPose(ref decisions[0], fish, xna.Vector3 vec(724, 0, -424), 1.57f, 30.0f, 8 * ball.RadiusMm, mission.CommonPara.MsPerCycle, times);
        }*/
        public void mydrible(ref Decision decision, RoboFish fish, Microsoft.Xna.Framework.Vector3 gate, Ball ball)
        {
            double fish_center_x = fish.PositionMm.X;//鱼的中心X轴坐标
            double fish_center_z = fish.PositionMm.Z;//鱼的中心Z轴坐标
            double fish_head_x = fish.PolygonVertices[0].X;//鱼头X轴坐标
            double fish_head_z = fish.PolygonVertices[0].Z;//鱼头Z轴坐标
            double ball_x = ball.PositionMm.X;//球的X轴坐标
            double ball_z = ball.PositionMm.Z;//球的Z轴坐标
            double bodyDirectionRad = fish.BodyDirectionRad;//当前鱼体方向
            double angularVelocityRadPs = fish.AngularVelocityRadPs;//当前角速度阀值
            double angle = Math.Atan((gate.Z - ball_z) / (gate.X - ball_x));//目标点和鱼的连线与X正方向的夹角
            if ((gate.X - ball_z) < 0.0)
            {
                if (angle > 0.0)
                {
                    angle -= 3.1415926535897931;//在一三象限时，
                }
                else
                {
                    angle += 3.1415926535897931;
                }
            }
            double num10 = ball_x - (40.0 * Math.Cos(angle));
            double num11 = ball_z - (40.0 * Math.Sin(angle));
            double num12 = Math.Sqrt(Math.Pow(fish_head_x - num10, 2.0) + Math.Pow(fish_head_z - num11, 2.0));
            double num13 = Math.Atan((num11 - fish_center_z) / (num10 - fish_center_x));
            if ((num10 - fish_center_x) < 0.0)
            {
                if (num13 > 0.0)
                {
                    num13 -= 3.1415926535897931;
                }
                else
                {
                    num13 += 3.1415926535897931;
                }
            }
            if (num12 < 10.0)
            {
                num13 = angle;
            }
            double num14 = num13 - bodyDirectionRad;
            if (num14 > 3.1415926535897931)
            {
                num14 -= 6.2831853071795862;
            }
            else if (num14 < -3.1415926535897931)
            {
                num14 += 6.2831853071795862;
            }
            double num15 = num12 / this.VTable[13];
            if (num15 > 2.0)
            {
                num15 = 2.0;
            }
            int index = 7;
            double num17 = num14 / num15;
            if (num14 > 0.0)
            {
                while ((index < 15) && (this.TTable[index] < num17))
                {
                    index++;
                }
                if ((angularVelocityRadPs / 2.0) > num14)
                {
                    index = 1;
                }
            }
            else
            {
                while ((index > 0) && (this.TTable[index] > num17))
                {
                    index--;
                }
                if ((angularVelocityRadPs / 2.0) < num14)
                {
                    index = 14;
                }
            }
            decision.TCode = index;
            if (num12 > 350.0)
            {
                if (Math.Abs(num14) < 0.19634954084936207)
                {
                    decision.VCode = 10;
                }
                else
                {
                    decision.VCode = 1;
                }
            }
            else if (Math.Abs(num14) < 0.19634954084936207)
            {
                decision.VCode = 5;
            }
            else
            {
                decision.VCode = 1;
            }
        }
        #endregion

        #region 判断是否在球门区域
        public bool inL(xna.Vector3 xna)//在左侧球门区域
        {
            if (Math.Abs(xna.Z) < 550 && xna.X < -950)
            {
                return true;
            }
            else
                return false;
        }

        public bool inR(xna.Vector3 xna)//在右侧球门区域
        {
            if (Math.Abs(xna.Z) < 550 && xna.X > 950)
            {
                return true;
            }
            else
                return false;
        }
        #endregion

        #region 新顶球策略
        public void BalltoPoint(ref Decision decision, RoboFish fish, Microsoft.Xna.Framework.Vector3 destPtMm, Ball ball)
        {//决策对象，仿真鱼对象，目标点，目标球
            double angle = Math.Atan((destPtMm.Z - ball.PositionMm.Z) / (destPtMm.X - ball.PositionMm.X));//目标点和球的连线与X正方向的夹角
            //double angle = StrategyHelper.Helpers.GetAngleDegree(destPtMm - ball.PositionMm);
            //将目标点和球的连线与X正方向的夹角规范化到(-PI,PI]
            //规范化之后目标角度为正表示目标方向在鱼体方向右边
            //规范化之后目标角度为负表示目标方向在鱼体方向左边
            if (destPtMm.X < ball.PositionMm.X)
            {
                if (angle > 0.0)
                {
                    angle -= 3.1415926535897931;// 规范化到(-PI,0)
                }
                else
                {
                    angle += 3.1415926535897931;// 规范化到(0,PI)
                }
            }
            double attackpointX = ball.PositionMm.X - (36.0 * Math.Cos(angle));//将球坐标强制转为球表面某点的X轴坐标
            double attackpointZ = ball.PositionMm.Z - (36.0 * Math.Sin(angle));//将球坐标强制转为球表面某点的Z轴坐标
            //以上两个变量确定一个点，即为强制改后的球坐标
            double distance = Math.Sqrt(Math.Pow(fish.PolygonVertices[0].X - attackpointX, 2.0) + Math.Pow(fish.PolygonVertices[0].Z - attackpointZ, 2.0));//鱼头到上述改后球坐标的距离
            double AngleofBalltoFish = Math.Atan((attackpointZ - fish.PositionMm.Z) / (attackpointX - fish.PositionMm.X));//鱼的头坐标到改后球坐标的夹角
            // 将鱼的头点坐标到改后球坐标的夹角规范化到(-PI,PI]
            // 规范化之后目标角度为正表示目标方向在鱼体方向右边
            // 规范化之后目标角度为负表示目标方向在鱼体方向左边
            if (attackpointX < fish.PositionMm.X)
            {
                if (AngleofBalltoFish > 0.0)
                {
                    AngleofBalltoFish -= 3.1415926535897931;// 规范化到(-PI,0)
                }
                else
                {
                    AngleofBalltoFish += 3.1415926535897931;// 规范化到(0,PI)
                }
            }
            if (distance < 10.0)
            {
                AngleofBalltoFish = angle;
            }
            double GoalAngle = AngleofBalltoFish - fish.BodyDirectionRad;// 鱼的坐标到改后球坐标的夹角方向与鱼体方向的差值（目标角度）
            // 将目标角度规范化到(-PI,PI]
            // 规范化之后目标角度为正表示目标方向在鱼体方向右边
            // 规范化之后目标角度为负表示目标方向在鱼体方向左边
            if (GoalAngle > 3.1415926535897931)
            {
                GoalAngle -= 6.2831853071795862;// 规范化到(-PI,0)
            }
            else if (GoalAngle < -3.1415926535897931)
            {
                GoalAngle += 6.2831853071795862;// 规范化到(-PI,0)
            }
            double TimeByDistance = distance / VTable[13];// 以最大速度游过目标距离所需的预计时间（距离预计时间）
            if (TimeByDistance > 2.0)// 距离预计时间超过一次档位切换所需平均时间则取为该时间（默认为2秒）
            {
                TimeByDistance = 2.0;
            }
            int tcode = 7;// 目标（角速度）档位初值置7,即直游
            double targetTcode = GoalAngle / TimeByDistance;//以距离预计时间游过目标角度所需平均角速度（目标角速度）
            if (GoalAngle > 0.0)
            {// 目标角度为正目标方向在鱼体方向右边需要给右转档位
                while ((tcode < 15) && (TTable[tcode] < targetTcode))
                {// 目标（转弯）档位对应的角速度值尚未达到目标角速度则调高目标（转弯）档位
                    tcode++;
                }
                if ((fish.AngularVelocityRadPs / 2.0) > GoalAngle)
                {// 当前角速度值绝对值过大 一次档位切换所需平均时间内能游过的角度超过目标角度
                    // 则给相反方向次大转弯档位
                    tcode = 1;
                }
            }
            else
            {// 目标角度为负目标方向在鱼体方向左边需要给左转档位（注意左转档位对应的角速度值为负值）
                while ((tcode > 0) && (TTable[tcode] > targetTcode))
                {// 目标（转弯）档位对应的角速度值尚未达到目标角速度则调低目标（转弯）档位
                    tcode--;
                }
                if ((fish.AngularVelocityRadPs / 2.0) < GoalAngle)
                {// 当前角速度值绝对值过大 一次档位切换所需平均时间内能游过的角度超过目标角度
                    // 则给相反方向次大转弯档位
                    tcode = 14;
                }
            }
            decision.TCode = tcode;//将决策的角速度设为最后确定的合适的值

            // 最大角速度取左转和右转最大角速度绝对值的均值
            float maxAngularV = (Math.Abs(TCodeAndAngularVelocityTable[0])
                + Math.Abs(TCodeAndAngularVelocityTable[14])) / 2;
            // 以最大角速度转过目标角度所需的预计时间（角度预计时间）
            double estimatedTimeByAngle = Math.Abs((double)(GoalAngle / maxAngularV));
            // 以角度预计时间游过目标距离所需平均速度值（目标速度）
            double targetVcode = distance / estimatedTimeByAngle;
            int vcode = 1;   // 目标（速度）档位初值置1
            while ((vcode < 10) && (VCodeAndVelocityTable[vcode] < targetVcode))
            {// 目标（速度）档位对应的速度值尚未达到目标速度则调高目标（速度）档位
                vcode++;
            }
            decision.VCode = vcode;
            if (Math.Abs(GoalAngle) > 0.392699081698724)
            {// 目标角度绝对值超过某一阈值，速度档位置次低进行小半径转弯
                decision.VCode = 1;
            }
            else if (Math.Abs(GoalAngle) > 0.19634954084936207 && Math.Abs(GoalAngle) < 0.392699081698724)
            {// 目标角度绝对值小于某一阈值，若此时距离较远速度档置较高高全速前进，否则置适中档位前进。
                if (distance > 250f)
                {
                    decision.VCode = 4;
                }
                else if (distance < 250f && distance > 100f)
                {
                    decision.VCode = 2;
                }
                else
                {
                    decision.VCode = 1;
                }
            }
            else if (Math.Abs(GoalAngle) < 0.19634954084936207)
            {// 目标角度绝对值小于某一阈值，若此时距离较远速度档置较高高全速前进，否则置适中档位前进。
                if (distance > 250f)
                {
                    decision.VCode = 10;
                }
                else
                {
                    decision.VCode = 4;
                }
            }
            else
            {
                decision.VCode = 1;
            }
        }
        #endregion
        #region HSgo
        public void HSgo(ref Decision decision, RoboFish fish, Microsoft.Xna.Framework.Vector3 destPtMm, Ball ball, float deviation, int ContactPoint)
        {//决策对象，仿真鱼对象，目标点，目标球
            //mission.EnvRef.Balls[0].PositionMm.X
            //mission.EnvRef.Balls[0].PositionMm.Z

            double angle = Math.Atan((destPtMm.Z - ball.PositionMm.Z) / (destPtMm.X - ball.PositionMm.X)) + deviation;//目标点和球的连线与X正方向的夹角
            //double angle = StrategyHelper.Helpers.GetAngleDegree(destPtMm - ball.PositionMm);
            //将目标点和球的连线与X正方向的夹角规范化到(-PI,PI]
            //规范化之后目标角度为正表示目标方向在鱼体方向右边
            //规范化之后目标角度为负表示目标方向在鱼体方向左边
            if (destPtMm.X < ball.PositionMm.X)
            {
                if (angle > 0.0)
                {
                    angle -= 3.1415926535897931;// 规范化到(-PI,0)
                }
                else
                {
                    angle += 3.1415926535897931;// 规范化到(0,PI)
                }
            }
            double attackpointX = ball.PositionMm.X - (36.0 * Math.Cos(angle));//将球坐标强制转为球表面某点的X轴坐标
            double attackpointZ = ball.PositionMm.Z - (36.0 * Math.Sin(angle));//将球坐标强制转为球表面某点的Z轴坐标
            //以上两个变量确定一个点，即为强制改后的球坐标
            double distance = Math.Sqrt(Math.Pow(fish.PolygonVertices[ContactPoint].X - attackpointX, 2.0) + Math.Pow(fish.PolygonVertices[ContactPoint].Z - attackpointZ, 2.0));//鱼头到上述改后球坐标的距离
            double AngleofBalltoFish = Math.Atan((attackpointZ - fish.PositionMm.Z) / (attackpointX - fish.PositionMm.X));//鱼的头坐标到改后球坐标的夹角
            // 将鱼的头点坐标到改后球坐标的夹角规范化到(-PI,PI]
            // 规范化之后目标角度为正表示目标方向在鱼体方向右边
            // 规范化之后目标角度为负表示目标方向在鱼体方向左边
            if (attackpointX < fish.PositionMm.X)
            {
                if (AngleofBalltoFish > 0.0)
                {
                    AngleofBalltoFish -= 3.1415926535897931;// 规范化到(-PI,0)
                }
                else
                {
                    AngleofBalltoFish += 3.1415926535897931;// 规范化到(0,PI)
                }
            }
            if (distance < 10.0)
            {
                AngleofBalltoFish = angle;
            }
            double GoalAngle = AngleofBalltoFish - fish.BodyDirectionRad;// 鱼的坐标到改后球坐标的夹角方向与鱼体方向的差值（目标角度）
            // 将目标角度规范化到(-PI,PI]
            // 规范化之后目标角度为正表示目标方向在鱼体方向右边
            // 规范化之后目标角度为负表示目标方向在鱼体方向左边
            if (GoalAngle > 3.1415926535897931)
            {
                GoalAngle -= 6.2831853071795862;// 规范化到(-PI,0)
            }
            else if (GoalAngle < -3.1415926535897931)
            {
                GoalAngle += 6.2831853071795862;// 规范化到(-PI,0)
            }
            double TimeByDistance = distance / VTable[13];// 以最大速度游过目标距离所需的预计时间（距离预计时间）
            if (TimeByDistance > 2.0)// 距离预计时间超过一次档位切换所需平均时间则取为该时间（默认为2秒）
            {
                TimeByDistance = 2.0;
            }
            int tcode = 7;// 目标（角速度）档位初值置7,即直游
            double targetTcode = GoalAngle / TimeByDistance;//以距离预计时间游过目标角度所需平均角速度（目标角速度）
            if (GoalAngle > 0.0)
            {// 目标角度为正目标方向在鱼体方向右边需要给右转档位
                while ((tcode < 15) && (TTable[tcode] < targetTcode))
                {// 目标（转弯）档位对应的角速度值尚未达到目标角速度则调高目标（转弯）档位
                    tcode++;
                }
                if ((fish.AngularVelocityRadPs / 2.0) > GoalAngle)
                {// 当前角速度值绝对值过大 一次档位切换所需平均时间内能游过的角度超过目标角度
                    // 则给相反方向次大转弯档位
                    tcode = 1;
                }
            }
            else
            {// 目标角度为负目标方向在鱼体方向左边需要给左转档位（注意左转档位对应的角速度值为负值）
                while ((tcode > 0) && (TTable[tcode] > targetTcode))
                {// 目标（转弯）档位对应的角速度值尚未达到目标角速度则调低目标（转弯）档位
                    tcode--;
                }
                if ((fish.AngularVelocityRadPs / 2.0) < GoalAngle)
                {// 当前角速度值绝对值过大 一次档位切换所需平均时间内能游过的角度超过目标角度
                    // 则给相反方向次大转弯档位
                    tcode = 14;
                }
            }
            decision.TCode = tcode;//将决策的角速度设为最后确定的合适的值

            // 最大角速度取左转和右转最大角速度绝对值的均值
            float maxAngularV = (Math.Abs(TCodeAndAngularVelocityTable[0])
                + Math.Abs(TCodeAndAngularVelocityTable[14])) / 2;
            // 以最大角速度转过目标角度所需的预计时间（角度预计时间）
            double estimatedTimeByAngle = Math.Abs((double)(GoalAngle / maxAngularV));
            // 以角度预计时间游过目标距离所需平均速度值（目标速度）
            double targetVcode = distance / estimatedTimeByAngle;
            int vcode = 1;   // 目标（速度）档位初值置1
            while ((vcode < 10) && (VCodeAndVelocityTable[vcode] < targetVcode))
            {// 目标（速度）档位对应的速度值尚未达到目标速度则调高目标（速度）档位
                vcode++;
            }
            decision.VCode = vcode;
            if (Math.Abs(GoalAngle) > 0.392699081698724)
            {// 目标角度绝对值超过某一阈值，速度档位置次低进行小半径转弯
                decision.VCode = 1;
            }
            else if (Math.Abs(GoalAngle) > 0.19634954084936207 && Math.Abs(GoalAngle) < 0.392699081698724)
            {// 目标角度绝对值小于某一阈值，若此时距离较远速度档置较高高全速前进，否则置适中档位前进。
                if (distance > 250f)
                {
                    decision.VCode = 4;
                }
                else if (distance < 250f && distance > 100f)
                {
                    decision.VCode = 2;
                }
                else
                {
                    decision.VCode = 1;
                }
            }
            else if (Math.Abs(GoalAngle) < 0.19634954084936207)
            {// 目标角度绝对值小于某一阈值，若此时距离较远速度档置较高高全速前进，否则置适中档位前进。
                if (distance > 250f)
                {
                    decision.VCode = 8;
                }
                else
                {
                    decision.VCode = 4;
                }
            }
            else
            {
                decision.VCode = 1;
            }
        }
        #endregion
        #endregion
    }
}
            #endregion